 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
VEXPro servo position control example
Description: This tutorial shows how to use the VEXPro embedded linux system with the libqwerk library that exposes device I/O to applications, in order to control the position of a servo.Tutorial Level: BEGINNER
Next Tutorial: VEXPro range motor control loop
Contents
The ultimate goal of an embedded linux system is typically to control robot actuators and to read sensors or cameras, and to pass robot data between the actuators and sensors and the ROS nodes that perform higher-level functions. Embedded linux systems typically have libraries which facilitate accessing connected peripherals. The VEXPro controller offers the libqwerk library for this purpose. Libqwerk provides a C++ interface to Vex Robotics peripherals including motors, ultrasonic range sensors, servos and more. The VEXPro is a general purpose controller and can also be used with 3rd-party peripherals, provided proper understanding of the interfaces is applied.
This tutorial shows how to integrate libqwerk control of a servo with a ROS subscription, such that ROS nodes can publish the desired servo position.
You should have already completed the rosserial and VEXPro software installation described here, which includes installing the VEXPro Terk IDE and running the quickstart Terk tutorial. If you haven't, you won't be able to run this tutorial (but reading through it may still be interesting).
The Code
Use your favorite editor or IDE to open the VEXProServoSubscribe.cpp tutorial code which is found in examples/VEXProServoSubscribe.cpp. You should see the following:
1 /*
2 * VEXProServoSubscribe.cpp
3 *
4 * Created on: Jul 12, 2012
5 * Author: bouchier
6 * Drives a servo or motor on VEXPro motor1 connection to the requested value: 0 - 255
7 * that is received on subscribed topic servo1
8 */
9
10 #include <ros.h>
11 #include <std_msgs/Int32.h>
12 #include <iostream>
13 #include <qeservo.h>
14 using namespace std;
15
16 ros::NodeHandle nh;
17 CQEServo &servo = CQEServo::GetRef();
18 char *rosSrvrIp = "192.168.11.9";
19
20 void messageCb(const std_msgs::Int32& servo1_msg){
21 int position = servo1_msg.data;
22 printf("Received subscribed servo position %d\n", position);
23 servo.SetCommand(0, position);
24 }
25 ros::Subscriber<std_msgs::Int32> sub("servo1", messageCb );
26
27 int main() {
28 //nh.initNode();
29 nh.initNode(rosSrvrIp);
30 nh.subscribe(sub);
31
32 while(1) {
33 sleep(1);
34 nh.spinOnce();
35 }
36 }
The code explained
Now let's break the code down.
You include <std_msgs/Int32.h> as it is the message type that will be published with the desired position in its low-order byte. (The code can be easily extended to address different servos by encoding them in an upper byte.)
You include <iostream> to declare the printf() function. Include <qeservo.h> to get the libqwerk servo control class. See the libqwerk servo class documentation for full details on all the methods and members related to controlling servos.
17 CQEServo &servo = CQEServo::GetRef();
A unique feature of libqwerk is that the application obtains a reference to the hardware control object (of which there is only one, and only one reference can be acquired). This reference is to the object which provides the hardware control methods. In this case, we get a reference to the servo control object.
Define the callback function that's invoked when a ROS node publishes to the servo1 topic. The function pulls the desired position out of the message and uses the CQEServo class' SetCommand() method to set the servo position. The first argument is the servo number; servo 0 corresponds to motor 1.
25 ros::Subscriber<std_msgs::Int32> sub("servo1", messageCb );
Declare the subscriber, as was done in the example subscriber of a previous tutorial, and pass it the name of the topic and the callback function.
main()
The rest of the program is the same as in the example subscriber tutorial: you initialize the node handle passing it the IP address of the rosserial_python proxy, subscribe, and then periodically run the spinOnce() function which polls the interface for incoming messages.
Testing the code
Plug a servo into the Motor1 port on the VEXPro. Note: Vex servos have plugs with male pins, whereas typical hobby servos have female pins. You can adapt one to the other with a 3-pin section of straight male header pins and an optional servo extender cable with the sheath around the pins removed.
As usual, make sure ros_core and serial_node.py are running. Build, download, and start the VEXProServoSubscribe binary on your embedded linux system.
root@qwerk:/opt/usr/bin# ./VEXProServoSubscribe Connecting to TCP server at 192.168.11.9:11411.... connected to server EmbeddedHardware.h: opened comm port successfully
Each time an embedded linux client connects using TCP/IP, rosserial_python creates a new socket server process, and creates a new node with a name that includes the port number being used for that connection. This is logged by the rosserial_python proxy.
Notice also that the subscription to the servo1 topic is logged by the rosserial_python proxy.
[INFO] [WallTime: 1348389339.177539] Established a socket connection from 192.168.11.8 on port 54788 [INFO] [WallTime: 1348389339.177657] Forking a socket server process [INFO] [WallTime: 1348389339.181057] launched startSocketServer waiting for socket connection [INFO] [WallTime: 1348389339.183572] starting ROS Serial Python Node serial_node-54788 [INFO] [WallTime: 1348389342.239358] Note: subscribe buffer size is 512 bytes [INFO] [WallTime: 1348389342.240773] Setup subscriber on servo1 [std_msgs/Int32]
Now publish servo values from 0 - 255 to topic servo1 and watch the servo move to the requested position.
$ rostopic pub -1 servo1 std_msgs/Int32 20 publishing and latching message for 3.0 seconds $ rostopic pub -1 servo1 std_msgs/Int32 200 publishing and latching message for 3.0 seconds
